package uebung03_w;

import java.awt.BasicStroke;
import java.awt.BorderLayout;
import java.awt.Color;
import java.awt.Dimension;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.RenderingHints;
import java.awt.event.MouseWheelEvent;
import java.awt.event.MouseWheelListener;
import java.awt.geom.AffineTransform;
import java.awt.geom.Ellipse2D;
import java.awt.geom.NoninvertibleTransformException;
import java.awt.geom.Point2D;
import java.awt.image.BufferedImage;
import java.util.ArrayList;

import javax.imageio.ImageIO;
import javax.swing.JComponent;
import javax.swing.JFrame;

import org.ejml.simple.SimpleEVD;
import org.ejml.simple.SimpleMatrix;

import tools.Arrow;
import tools.ColorConversions;
import tools.Derivative;
import tools.Gauss;
import tools.Image;

public class KLT02w {
	public static void main(String[] args) throws Exception {
		
		final BufferedImage bi1 = ImageIO.read(KLT02w.class.getClassLoader().getResource("res/lena1.png"));
		final BufferedImage bi2 = ImageIO.read(KLT02w.class.getClassLoader().getResource("res/lena2.png"));
		
		final int w = bi1.getWidth(), h = bi1.getHeight();
		
		double [] I1 = Image.getScalarLuminance(ColorConversions.getFastLuminance(bi1.getRGB(0, 0, w, h, new int[w*h], 0, w), new int[w*h]), new double[w*h]);
		double [] I2 = Image.getScalarLuminance(ColorConversions.getFastLuminance(bi2.getRGB(0, 0, w, h, new int[w*h], 0, w), new int[w*h]), new double[w*h]);
		
		double[] dtI = Derivative.dt(I1, I2, new double[w*h]);
		double[] dxI2 = Derivative.dx(I2, w, h, new double[w*h]);
		double[] dyI2 = Derivative.dy(I2, w, h, new double[w*h]);
		
		final int step = 20, windowR = 20;
		final ArrayList<Point2D> hs = new ArrayList<Point2D>(), ps = new ArrayList<Point2D>(), es = new ArrayList<Point2D>();
		
		SimpleMatrix lgs = new SimpleMatrix(2, 2);
		SimpleMatrix dts = new SimpleMatrix(2, 1);
		
		for (int y = windowR; y<h-windowR; y+=step) {
			for (int x = windowR; x<w-windowR; x+=step) {
				
				double dxdx = 0, dydx = 0, dydy = 0, dtdx = 0, dtdy = 0, sumWeight = 0;
				
				for (int k = 0 ; k<2*windowR+1; k++) {
					int j = k-windowR;
					for (int l = 0; l<2*windowR+1; l++) {
						int i = l-windowR;
						
						int index = i+x+(y+j)*w;
						
						double dx = -dxI2[index]*0.5, dy = dyI2[index]*0.5, dt = dtI[index];
						
						double weight = Gauss.gaussian1d(i*i+j*j, (windowR+0.5)/3.); 
						
						weight = 1;
						
						dxdx += dx*dx*weight; 
						dydx += dy*dx*weight;
						dydy += dy*dy*weight;
						
						dtdx -= dt*dx*weight;
						dtdy -= dt*dy*weight;
						
						sumWeight += weight;
					}
				}
				
				dxdx /= sumWeight;
				dydx /= sumWeight;
				dydy /= sumWeight;
				
				dtdx /= sumWeight;
				dtdy /= sumWeight;

				double denominator = (dydx*dydx-dxdx*dydy);
				
				lgs.set(0, 0, dxdx);
				lgs.set(1, 0, dydx);
				lgs.set(0, 1, dydx);
				lgs.set(1, 1, dydy);
				
				if (Double.compare(0, denominator)!=0 && Double.compare(lgs.determinant(), 0)!=0) {
					
					dts.set(0, 0, dtdx);
					dts.set(1, 0, dtdy);
					
					SimpleMatrix result = lgs.solve(dts);
					SimpleEVD<SimpleMatrix> eigenValues = lgs.eig();
					
					es.add(new Point2D.Double(eigenValues.getEigenvalue(0).real, eigenValues.getEigenvalue(1).real));
					
					double hx = result.get(0, 0), hy = result.get(1, 0);
				
					ps.add(new Point2D.Double(x,y));
					hs.add(new Point2D.Double(hx,hy));

				}
				
			}
		}
		
		double max = 0;
		for (int i = 0; i<es.size(); i++) {
			Point2D p = es.get(i);
			max = Math.max(max, Math.max(p.getX(), p.getY()));
		}
		final double minQuality = max*0.1;
		
		
		final ArrayList<Point2D> normals = new ArrayList<Point2D>();
		final ArrayList<Double> distances = new ArrayList<Double>();
		
		for (int i = 0; i<hs.size(); i++) {
			Point2D eig = es.get(i);
			if (Math.max(eig.getX(), eig.getY())>minQuality) {
				
				Point2D a = ps.get(i), b = hs.get(i);
				
				double norml = Math.sqrt(b.getX()*b.getX()+b.getY()*b.getY());
				
				Point2D normal = new Point2D.Double(b.getY()/norml, -b.getX()/norml);
				
				normals.add(normal);
				
				distances.add(a.getX()*normal.getX()+a.getY()*normal.getY());
			}
		}
		
		double xx = 0, yx = 0, yy = 0, tx = 0, ty = 0;
		
		for (int i = 0; i<normals.size(); i++) {
			Point2D norml = normals.get(i);
			double a1 = norml.getX(), a2 = norml.getY(), c = distances.get(i);
			xx += a1*a1;
			yx += a1*a2;
			yy += a2*a2;
			tx += a1*c;
			ty += a2*c;
		}

		lgs.set(0, 0, xx);
		lgs.set(0, 1, yx);
		lgs.set(1, 0, yx);
		lgs.set(1, 1, yy);
		
		SimpleMatrix av = new SimpleMatrix(2, 1);
		
		av.set(0, 0, tx);
		av.set(1, 0, ty);
		
		SimpleMatrix vp = lgs.solve(av);
		
		System.out.println(vp.get(0, 0)+" "+vp.get(1, 0));
		
		final Point2D vanishingPoint = new Point2D.Double(vp.get(0,0), vp.get(1, 0));
		
		JFrame ff = new JFrame();
		ff.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
		
		final AffineTransform zoom = AffineTransform.getScaleInstance(1, 1);
		
		final JComponent canvas = new JComponent() {
			{setPreferredSize(new Dimension(w+w,h));}
			AffineTransform bi2transform = AffineTransform.getTranslateInstance(bi1.getWidth(), 0);
			Arrow arrow = new Arrow();
			Ellipse2D vp = new Ellipse2D.Double();
			
			@Override
			public void paint(Graphics g) {
				Graphics2D g2 = (Graphics2D)g;
				g2.transform(zoom);
				g2.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON);
				g2.setRenderingHint(RenderingHints.KEY_RENDERING, RenderingHints.VALUE_RENDER_QUALITY);
				g2.setRenderingHint(RenderingHints.KEY_STROKE_CONTROL, RenderingHints.VALUE_STROKE_PURE);
				
				g2.drawImage(bi1, 0, 0, this);
				g2.drawImage(bi2, bi2transform, this);
				
				for (int i = 0; i<hs.size(); i++) {
					Point2D eig = es.get(i);
					if (Math.max(eig.getX(), eig.getY())>minQuality) {
						
						Point2D p1 = ps.get(i), p2 = hs.get(i);
						Point2D dst = new Point2D.Double(p1.getX()+p2.getX(), p1.getY()+p2.getY());
						
						arrow.setArrow(p1, dst, 3, 9);
						
						g2.setColor(Color.magenta);
						g2.fill(arrow);
						g2.setColor(Color.green);
						g2.draw(arrow);
						
						arrow.setArrow(p1, dst, 3, 9);
						g2.setColor(Color.magenta);
						g2.fill(bi2transform.createTransformedShape(arrow));
						g2.setColor(Color.green);
						g2.draw(bi2transform.createTransformedShape(arrow));
						
					}
				}
				vp.setFrameFromCenter(vanishingPoint, new Point2D.Double(vanishingPoint.getX()-3, vanishingPoint.getY()-3));
				
				g2.setColor(Color.red);
				
				g2.draw(vp);
				g2.draw(bi2transform.createTransformedShape(vp));
				
				super.paint(g);
			}
		};
		canvas.addMouseWheelListener(new MouseWheelListener() {
				
				@Override
				public void mouseWheelMoved(MouseWheelEvent e) {
					
					double scale = Math.pow(1.01, -e.getWheelRotation());

					try {
						Point2D p = new Point2D.Double(e.getX(), e.getY()); 
						zoom.inverseTransform(p, p);
						zoom.concatenate(AffineTransform.getTranslateInstance(p.getX(), p.getY()));
						zoom.concatenate(AffineTransform.getScaleInstance(scale, scale));
						zoom.concatenate(AffineTransform.getTranslateInstance(-p.getX(), -p.getY()));
					}
					catch (NoninvertibleTransformException nte ) {
						nte.printStackTrace();
					}
					canvas.repaint();
				}
			});
	
		ff.add(canvas, BorderLayout.CENTER);
		
		ff.pack();
		ff.setVisible(true);
		
	}
}
